#ifndef CONTROLLER_3D_POSPID_H_
#define CONTROLLER_3D_POSPID_H_

#include <math.h>
#include <signal.h>
#include <time.h>
#include <sys/time.h>

#include <common/mavlink.h>

#include "controller_posPID.h"
#include "../navigator/waypoint_list.h"
#include "../navigator/ref_navigator.h"

#ifndef HAVE_DEF_PI
#define HAVE_DEF_PI
#define PI 3.1415926
#endif

#ifndef HAVE_STRUCT_REF_VELOCITY_T
#define HAVE_STRUCT_REF_VELOCITY_T
typedef struct _ref_velocity_t
{
	float vx;
	float vy;
	float vz;
}ref_velocity_t;
#endif

#ifndef HAVE_STRUCT_D3_CONTROL_T
#define HAVE_STRUCT_D3_CONTROL_T
typedef struct _d3_control_t
{
	ref_velocity_t velocity;
	float yaw_ref;
}d3_control_t;
#endif

#ifndef HAVE_STRUCT_D3_PID_T
#define HAVE_STRUCT_D3_PID_T
typedef struct _d3_PID_t
{
	PID_kit_t PID_x;
	PID_kit_t PID_y;
	PID_kit_t PID_z;
}d3_PID_t;
#endif

class controller_3D_posPID_c
{
public:
	ref_point_t ref;
	ref_point_t feedback;
	
	controller_posPID_c controller_x;
	controller_posPID_c controller_y;
	controller_posPID_c controller_z;
	
	d3_control_t d3_ctrl;

	controller_3D_posPID_c(float dT);
	controller_3D_posPID_c();
	~controller_3D_posPID_c();

	void set(d3_PID_t d3_pid);
	void set_yaw(float yaw_degree);
	int* calc();
	
	
};




#endif
